import math
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    """
    内环姿态控制器
    外环轨迹跟踪器
    """

    state_controller = Node(
        package="controller",
        executable="tri_pid",
        name="controller",
        output="screen",
        parameters=[
            {"gyro_pid": [0.1, 0.5, 0.0, 100.0]},
            {"angle_pid": [5.0, 0.0, 0.0, 100.0]},
            {"speed_pid": [0.0, 0.0, 0.0, 100.0]},
            {"rotate_pid": [0.2, 1.0, 0.0, 100.0]},
            {"state_rate": 100.0},
        ],
    )

    follow_mpc_node = Node(
        package="controller_py",
        executable="mpc_path_tracker",
        name="mpc_path_tracks",
        output="screen",
        parameters=[
            {"max_gyro":30 / 180.0 * math.pi},
            {"mat_q": [10.0, 10.0, 10.0, 10.0]},
            {"mat_r": [1.0, 1.0]},
        ],
        remappings=[("odom", "sp_odom")],
    )

    return LaunchDescription(
        [
            #DeclareLaunchArgument("vec", default_value="1.0"),
            state_controller,
            follow_mpc_node,
        ]
    )
